﻿#include "capcontrol.h"

CCapControl*CCapControl::_pInstance = nullptr;//实现单例模式

CCapControl::CCapControl(QObject *parent) : QThread(parent)
{
    _i64frameId = 0;

    _i32FrameCnt = 0;

    _bStartRcvData = false;

    m_bConnect = false;

    _i32GetParamCnt = 0;
#if 0
    _pStatusTimer = new QTimer(this);
    connect(_pStatusTimer,&QTimer::timeout,this,[=](){

        if(m_bConnect == false)
        {
            return;
        }
        if(_i32GetParamCnt == 0)
        {
            getAxisParam(0x0,0x8A);
        }

        if(_i32GetParamCnt == 1)
        {
            getAxisParam(0x0,0x85);
        }

        if(_i32GetParamCnt == 2)
        {
            getAxisParam(0x0,0x9A);
        }

        _i32GetParamCnt++;
        if(_i32GetParamCnt > 2)
        {
            _pStatusTimer->stop();
        }

    });
    _pStatusTimer->start(500);
#endif
}

CCapControl::~CCapControl()
{

}

CCapControl* CCapControl::getInstance()
{
    if(_pInstance == nullptr)
    {
        _pInstance = new CCapControl();
    }

    return _pInstance;
}

/**
* @brief
* @note
* 设置pid参数
*/
void CCapControl::getAxisParam(int i32AxisNo,int i32Cmdid)
{
    st_pid_frame l_paramInput;
    l_paramInput.datatype = 0x24;
    l_paramInput.cmdid = i32Cmdid;
    l_paramInput.checksum = caclchecksum((uchar*)&l_paramInput,sizeof(l_paramInput)-1);
    addSendData((char*)&l_paramInput,sizeof(l_paramInput));
}

/**
* @brief
* @note
* 设置pid参数
*/
bool CCapControl::setAxisParam(int i32AxisNo,int i32Cmdid,float lfparam)
{
    bool bRel = false;
    st_pid_frame l_paramInput;

    l_paramInput.cmdid = i32Cmdid;

    fourchar l_data;
    l_data.ui32data = lfparam;

    l_paramInput.data[0] = l_data.data[3];
    l_paramInput.data[1] = l_data.data[2];
    l_paramInput.data[2] = l_data.data[1];
    l_paramInput.data[3] = l_data.data[0];
    l_paramInput.checksum = caclchecksum((uchar*)&l_paramInput,sizeof(l_paramInput)-1);

    addSendData((char*)&l_paramInput,sizeof(l_paramInput));

    QThread::msleep(20);
    //查询一次参数
    getAxisParam(i32AxisNo,i32Cmdid);

    return true;
}
/**
* @brief 门限参数设置
* @note
*
*/
bool CCapControl::getThresholdParam(int i32AxisNo,int i32Cmdid)
{
    st_thresholdParam l_st_thresholdParam;
    l_st_thresholdParam.datatype = 0x14;

    l_st_thresholdParam.cmdid = i32Cmdid;

    l_st_thresholdParam.checksum = caclchecksum((uchar*)&l_st_thresholdParam,sizeof(l_st_thresholdParam)-1);

    addSendData((char*)&l_st_thresholdParam,sizeof(l_st_thresholdParam));
    QThread::msleep(20);

    getThresholdParam(i32AxisNo,i32Cmdid);

    return true;
}

/**
* @brief 门限参数设置
* @note
*
*/
bool CCapControl::setThresholdParam(int i32AxisNo,int i32Cmdid,int i32Value,int datalen)
{
    bool bRel = false;
    fourchar l_data;
    l_data.i32data = i32Value;

    st_thresholdParam l_st_thresholdParam;
    l_st_thresholdParam.datatype = 0x13;

    l_st_thresholdParam.cmdid = i32Cmdid;
    //高低位反转
    l_st_thresholdParam.data[0] = l_data.data[3];
    l_st_thresholdParam.data[1] = l_data.data[2];
    l_st_thresholdParam.data[2] = l_data.data[1];
    l_st_thresholdParam.data[3] = l_data.data[0];

    l_st_thresholdParam.checksum = caclchecksum((uchar*)&l_st_thresholdParam,sizeof(l_st_thresholdParam)-1);

    addSendData((char*)&l_st_thresholdParam,sizeof(l_st_thresholdParam));

    return true;
}
/**
* @brief 解锁指令
* @note
*
*/
void CCapControl::setUnlockCmd(int cmdid,int value)
{
    st_unlockcmd l_st_unlockcmd;

    //l_st_unlockcmd.frame_cnt = _i16UnlockFrameCnt;
    l_st_unlockcmd.frame_cmdid = cmdid;
    l_st_unlockcmd.unlocktime = value;
    l_st_unlockcmd.checksum = caclchecksum((uchar*)&l_st_unlockcmd,sizeof(l_st_unlockcmd)-1);
    addSendData((char*)&l_st_unlockcmd,sizeof(l_st_unlockcmd));

//    _i16UnlockFrameCnt++;
}
/**
* @brief 握手指令
* @note
*
*/
void CCapControl::setAxisHandShakeParam(int i32AxisNo,int i32value,int datalen)
{
    st_handshake l_st_handshake;
    memset(&l_st_handshake,0,sizeof(l_st_handshake));

    l_st_handshake.frame_header = 0xC1;
    l_st_handshake.frame_type = 0x44;
//    l_st_handshake.frame_cnt = _i16HandShakeCnt;
    l_st_handshake.azimuth_workmode = 0x2A;
    l_st_handshake.azimuth_datatype = 0xFE;
    l_st_handshake.pitch_workmode = 0x2A;
    l_st_handshake.pitch_datatype = 0xFE;

    fourchar l_four_data;
    l_four_data.i32data = i32value;

    if(i32AxisNo == M_AXIS_AZI)
    {
        l_st_handshake.azimuth_datatype = 0xAA;
        l_st_handshake.azimuth_data[0] = l_four_data.data[2];
        l_st_handshake.azimuth_data[1] = l_four_data.data[1];
        l_st_handshake.azimuth_data[2] = l_four_data.data[0];
    }
    if(i32AxisNo == M_AXIS_PICTH)
    {
        l_st_handshake.pitch_datatype = 0xAA;
        l_st_handshake.pitch_data[0] = l_four_data.data[2];
        l_st_handshake.pitch_data[1] = l_four_data.data[1];
        l_st_handshake.pitch_data[2] = l_four_data.data[0];
    }

    l_st_handshake.checksum = caclchecksum((uchar*)&l_st_handshake,sizeof(l_st_handshake)-1);


    addSendData((char*)&l_st_handshake,sizeof(l_st_handshake));

//    _i16HandShakeCnt++;
}



/**
* @brief
* @note
*
*/
void CCapControl::startGroudTest(int i32AxisNo,int i32Mode,int data0,int data1)
{
    fourchar l_data0;
    l_data0.i32data = data0;
    fourchar l_data1;
    l_data1.i32data = data1;

    st_groundTestParam l_st_groundTestParam;
    l_st_groundTestParam.cmdid = i32Mode;

    l_st_groundTestParam.data1[0] = l_data0.data[3];
    l_st_groundTestParam.data1[1] = l_data0.data[2];
    l_st_groundTestParam.data1[2] = l_data0.data[1];
    l_st_groundTestParam.data1[3] = l_data0.data[0];

    l_st_groundTestParam.data2[0] = l_data1.data[3];
    l_st_groundTestParam.data2[1] = l_data1.data[2];
    l_st_groundTestParam.data2[2] = l_data1.data[1];
    l_st_groundTestParam.data2[3] = l_data1.data[0];

    l_st_groundTestParam.checksum = caclchecksum((uchar*)&l_st_groundTestParam,sizeof(l_st_groundTestParam)-1);
    addSendData((char*)&l_st_groundTestParam,sizeof(l_st_groundTestParam));
}

/**
* @brief
* @note
*
*/
uchar CCapControl::caclchecksum(uchar *pdata, int i32datalen)
{
    if(pdata == nullptr)
    {
        return 0;
    }

    uchar l_data = pdata[0];
    for(int i = 1; i < i32datalen; i++)
    {
        l_data = l_data^pdata[i];
    }
    l_data = ~l_data;
    return l_data;

}


/**
* @brief
* @note
*
*/
void CCapControl::addSendData(char* pdata,int nlen)
{
    QByteArray l_sendata;
    l_sendata.resize(nlen);
    memcpy(l_sendata.data(),pdata,nlen);

    _senddataLocker.lock();
    _senddatalist.append(l_sendata);
    _senddataLocker.unlock();
}



/**
* @brief 解析UDP数据包
* @note
*
*/
void CCapControl::parseRcvData(char* pdata,int nlen)
{
    int i32FrameType = -1;
    int nFramedatalen = 13;
    while(1)
    {
        //找帧头  帧尾 识别出一个完整的帧数据
        //发送给外部进行处理
        if(m_sRcvData.size() < nFramedatalen)
        {
            return;
        }

        int nStartPos = -1;
        uchar* pdata = (uchar*)(m_sRcvData.data());

        for(int i = 0; i < m_sRcvData.size()-1; i++)
        {
            if(pdata[i]== 0xC1 && pdata[i+1]== 0x11&& pdata[i+2]== 0x14)
            {
                nStartPos = i;
                i32FrameType = 0x01;//参数读取
                break;
            }

            if(pdata[i]== 0xC1 && pdata[i+1]== 0x11&& pdata[i+2]== 0x24)
            {
                nStartPos = i;
                i32FrameType = 0x02;//位置环/速度环/电流环参数注入状态回传
                break;
            }

            if(pdata[i]== 0xC1 && pdata[i+1]== 0x11&& pdata[i+2]== 0x44)
            {
                nStartPos = i;
                i32FrameType = 0x03;//驱动器上传数据协议
                break;
            }
        }
        if(nStartPos == -1 || i32FrameType == -1)
        {
            return;
        }

        //存在帧头数据 且 数据长度大于帧长度
        if(m_sRcvData.size() - nStartPos >= nFramedatalen)
        {
            if(i32FrameType == 0x01)
            {
                st_thresholdParam l_rcvdata;
                memcpy(&l_rcvdata,pdata+nStartPos,nFramedatalen);
                QVariant l_var;
                l_var.setValue(l_rcvdata);
                emit signal_rcvthresholddata(l_var);
            }

            if(i32FrameType == 0x02)//pid参数
            {
                st_pid_frame l_st_paraminput;
                memcpy(&l_st_paraminput,pdata+nStartPos,nFramedatalen);
                QVariant l_var;
                l_var.setValue(l_st_paraminput);
                emit signal_rcvpiddata(l_var);
            }

            if(i32FrameType == 0x03)//实时反馈数据
            {
                st_loopbackdata l_st_loopbackdata;
                memcpy(&l_st_loopbackdata,pdata+nStartPos,nFramedatalen);

                l_st_loopbackdata.i16CurrentA = sw16(l_st_loopbackdata.i16CurrentA);
                l_st_loopbackdata.i16CurrentB = sw16(l_st_loopbackdata.i16CurrentB);
                l_st_loopbackdata.i16QSetdata = sw16(l_st_loopbackdata.i16QSetdata);
                l_st_loopbackdata.i16QRespdata = sw16(l_st_loopbackdata.i16QRespdata);
                l_st_loopbackdata.i16Current = sw16(l_st_loopbackdata.i16Current);

                l_st_loopbackdata.i16SpeedSetdata = sw16(l_st_loopbackdata.i16SpeedSetdata);
                l_st_loopbackdata.i16SpeedRespdata = sw16(l_st_loopbackdata.i16SpeedRespdata);
                l_st_loopbackdata.i32PosSetdata = sw32(l_st_loopbackdata.i32PosSetdata);
                l_st_loopbackdata.i32PosRespdata = sw32(l_st_loopbackdata.i32PosRespdata);

                l_st_loopbackdata.i16MotorTemplate = sw16(l_st_loopbackdata.i16MotorTemplate);
                l_st_loopbackdata.i16IPMTemplate = sw16(l_st_loopbackdata.i16IPMTemplate);


                QVariant l_var;
                l_var.setValue(l_st_loopbackdata);
                emit signal_showloopdata(l_var);
            }

            //移除处理过的数据
            m_sRcvData.remove(0,nStartPos + nFramedatalen);
        }
        else
        {
            return;
        }

    }




    if(nlen != 98)
    {
        return;
    }

    if((uchar)(pdata[38]) != 0xC1 || (uchar)(pdata[39]) != 0x91)
    {
        return;
    }

    st_rcvdata l_rcv_data;
    memcpy(&l_rcv_data,pdata+38,60);

    QVariant l_var;
    l_var.setValue(l_rcv_data);

//    _i32FrameCnt++;
//    if(_i32FrameCnt %50 ==0)
//    {
//        emit signal_rcvdata(l_var);
//    }

}

/**
* @brief
* @note
*
*/
bool CCapControl::startRcvData(QString sComNo,int bandNum)
{
    _sComNo = sComNo;
    _i32BandNum = bandNum;

    this->start();

    QTime l_time;
    l_time.start();
    while(l_time.elapsed() < 1000)
    {
        QApplication::processEvents(QEventLoop::AllEvents,100);
        if(_bStartRcvData == true)
        {
            return true;
        }
    }

    return false;
}



/**
* @brief
* @note
*
*/
void CCapControl::run()
{
    m_pSerialPort = new QSerialPort();
    m_pSerialPort->setPortName(_sComNo);
    m_pSerialPort->setBaudRate(_i32BandNum);
    m_pSerialPort->setDataBits(QSerialPort::Data8);
    m_pSerialPort->setStopBits(QSerialPort::OneStop);
    m_pSerialPort->setParity(QSerialPort::NoParity);
    m_bConnect = m_pSerialPort->open(QIODevice::ReadWrite);

    if(m_bConnect == false)
    {
        return;
    }

    _bStartRcvData = true;
    while(_bStartRcvData)
    {
        _senddataLocker.lock();
        for(QByteArray& item:_senddatalist)
        {
            int nlen = m_pSerialPort->write(item.data(),item.size());
            if(nlen != item.size())
            {
                continue;
            }
            QThread::msleep(5);
        }
        _senddatalist.clear();
        _senddataLocker.unlock();

        while (m_pSerialPort->waitForReadyRead(5))
        {
            QByteArray l_rcvdata = m_pSerialPort->readAll();
            m_sRcvData.append(l_rcvdata);
        }

        parseRcvData(m_sRcvData.data(),m_sRcvData.size());

        QThread::msleep(1);
    }

}
